package pl.gigiel.behaviour.util;

import com.jme.math.Quaternion;
import com.jme.math.Vector3f;

public class CoordinateSystemTransformer {
	public static Vector3f transform(final Vector3f newY, final Vector3f vector) {
		Vector3f result = new Vector3f(vector);
		Quaternion worldRotation = rotateUpTo(newY);
		worldRotation.inverse().mult(result, result);
		return result;
	}

	public static Quaternion rotateUpTo(Vector3f newUp) {
		// First figure out the current up vector.
		Vector3f upY = new Vector3f(Vector3f.UNIT_Y);
		Quaternion localRotation = new Quaternion();
		localRotation.multLocal(upY);

		// get angle between vectors
		float angle = upY.angleBetween(newUp);

		// figure out rotation axis by taking cross product
		Vector3f rotAxis = upY.crossLocal(newUp).normalizeLocal();

		// Build a rotation quat and apply current local rotation.
		Quaternion q = new Quaternion();
		q.fromAngleNormalAxis(angle, rotAxis);
		q.mult(localRotation, localRotation);
		return q;
	}
}
